/*
 * @Author: tianchunyu
 * @Date: 2020-05-16 11:07:13
 * @LastEditTime: 2020-05-17 17:57:38
 * @LastEditors: Please set LastEditors
 * @Description:  the drive of The brush rises and falls
 * @FilePath: \app\xp\driver\syntron\turners_s.c
 */ 
#include "turners_s.h"
static TURNERS_class turners={0};
static u8 ifturnstop = 0;
/**
 * @description: Configure some parameters
 * @param {type} void 
 * @return: 
 *          void 
 */
void xp_turners_config(void)
{
    turners.init = 0;
    turners.angle = 0;
    turners.check_zero = 0;
    turners.run =0;
    turners.encode =0;
    turners.hinge_hander = &synro_brush_hinge_hander;
    turners.hinge_hander->config.drivemode = syn_pos_abs_speed;
    synro_brush_hinge_hander.config.acc_time = 1000; //默认加减速都是800
    synro_brush_hinge_hander.config.dcc_time = 1000;
}


/**
 * @description: The motor drive for lifting the brush is initialized
 * @param  port :uart port
 *         speed :baundspeed 
 *         slave id :slave id 
 * @return: 
 *          0 success
 *          -1 fail
 */

int xp_turners_init(u8 port,u32 speed,u8 slave_id)
{
    int ret =0;
    if(turners.init == 1)
    {
        LOG_SYNRON("turners has initialized !!!");
        return SYNRON_SUCCESS;
    }
    xp_turners_config();
    ret = synron_hander_init(turners.hinge_hander,2,slave_id,port,speed);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON(" synron_hander_init turners.hinge_hander fail !!");
        return SYNRON_NOR_ERR;
    }
    /*
    线没接先注释掉
    ret = synron_setemg_pinnum(turners.hinge_hande,T_IPUT_DRIVE_EMG_PIN);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("synron_setemg_pinnum turners.hinge_hande fail !!");
        return SYNRON_NOR_ERR;
    }
    */
    ret = xp_synron_position_init(turners.hinge_hander,syn_pos_abs_speed);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("xp_synron_position_init turners.hinge_hander fail !");
        return SYNRON_COMMUN_ERR;
    }
    ret = xp_turners_acc_dcc_time(synro_brush_hinge_hander.config.dcc_time);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("xp_turners_acc_dcc_time fail !");
        return SYNRON_COMMUN_ERR;
    }
    ret = synron_write_register(turners.hinge_hander,PARAMETER_POS_BIAS_SETVALUE,0);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("synron_write_register  turners.hinge_hander PARAMETER_POS_BIAS_SETVALUE fail !");
        return SYNRON_COMMUN_ERR;
    }
    ret = synron_position_re_encode_setvalue(turners.hinge_hander,0);
    if(ret != 0)
    {
        LOG_SYNRON("synron_position_re_encode_setvalue turners.hinge_hander fail !");
        return -1;
    }    
    ret = synron_position_re_locacmd_setvalue(turners.hinge_hander,0);
    if(ret != 0)
    {
        LOG_SYNRON("synron_position_re_locacmd_setvalue turners.hinge_hander fail !");
        return -1;
    }    
    turners.init = 1;
    return SYNRON_SUCCESS;
}


/**
 * @description: xp_turners_convert_angle Convert the Angle into a pulse
 * @param angle: Angle  unit  degree notice it's not radians
 * @return: pulse 
 *        
 */
int xp_turners_convert_angle(double angle)
{
    return ((s32)(angle*TURNERS_part*TURNERS_ratio/360));
}

/*
 * @description: 这个函数只针对绝对位置模式下用时间控制
 * @param {type} 
 * @return: 
 */
int  xp_turners_move_time(double angle ,u16 times_ms)
{
    int ret =0;
    s32 des_pulse = 0;
    if(angle < TURNERS_MIX_ANGLE || angle > TURNERS_MAX_ANGLE)
    {
        LOG_SYNRON("angle is out of range and it is %d ",angle);
        return SYNRON_NOR_ERR;
    }
    if(times_ms < 5)
    {
        LOG_SYNRON("times_ms is too short %d",times_ms);
        return SYNRON_NOR_ERR;
    }
    des_pulse = xp_turners_convert_angle(angle);
    LOG_SYNRON("des_pulse %d  angle %f",des_pulse,angle);
    ret = synron_position_setvalue(turners.hinge_hander,des_pulse,times_ms);//A motor
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("synron_position_setvalue turners.hinge_hander  error ! ");
        return SYNRON_COMMUN_ERR;
    }
    /*
        追踪没写有机械急停
    */ 
   return SYNRON_SUCCESS;
}



/*
 * @description: 这个函数只针对绝对位置模式下用速度控制
 * @param {type} agnle : -10-90度 不是弧度 
 *               speed:度/秒   度每秒
 * @return: 
 */
int  xp_turners_move_speed(double angle ,float speed)
{
    int ret =0;
    s32 des_pulse = 0; 
    u16 motor_speed = 0;//  unit prm  每分钟多少转
    if(angle < TURNERS_MIX_ANGLE || angle > TURNERS_MAX_ANGLE)
    {
        LOG_SYNRON("angle is out of range and it is %f ",angle);
        return SYNRON_NOR_ERR;
    }
    if(speed < 0)
    {
       LOG_SYNRON("(speed < 0) error");       
       return -1;        
    }
    if(speed>60)
    {
        speed = 60;
        LOG_SYNRON("WARNING !!!\n\r,speed max is 60 in consideration of safety");
    }
    des_pulse = xp_turners_convert_angle(angle);
    LOG_SYNRON("des_pulse %d  angle %f",des_pulse,angle);
    motor_speed = (u16)(speed *(TURNERS_LIFTS_MOTORS));
    LOG_SYNRON("speed %f du/s  motor_speed %d prm",speed,motor_speed);
    ret = synron_position_setvalue(turners.hinge_hander,des_pulse,motor_speed);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("synron_position_setvalue turners.hinge_hander  error ! ");
        xp_turners_error_anction();
        return SYNRON_COMMUN_ERR;
    }
    /*
        追踪没写有机械急停
    */ 
   ifturnstop = 0;
   while(1)
   {
        if(READADD_32_ERR==xp_turners_get_encode())
        {
            xp_turners_error_anction();
           return SYNRON_COMMUN_ERR;
        }
        if(fabs(turners.angle - angle)<1)
        {
            LOG_SYNRON("reach the specific angle");
            break;
        }
        if(xp_io_read_input_pin(1,TURNERS_UP_LIMIT_POSITION)==0)
        {
             LOG_SYNRON("exceptional situation  and reach the up limit position");
             xp_turners_set_cur_angle(TURNERS_MAX_ANGLE);
             return SYNRON_NOR_ERR;
        }
        if(xp_io_read_input_pin(1,TURNERS_DOWN_LIMIT_POSITION)==0)
        {
             LOG_SYNRON("exceptional situation  and reach the up limit position");
             xp_turners_set_cur_angle(TURNERS_MIX_ANGLE);
             return SYNRON_NOR_ERR;
        }
        if(ifturnstop == 1)
        {
            LOG_SYNRON("stop touched !!!");
            ifturnstop = 0;
            return SYNRON_NOR_ERR;
        }
        LOG_SYNRON("des_pulse %d  des_angle %f  and  cur_angle %f",des_pulse,angle,turners.angle);
        aos_msleep(200);
   }
   return SYNRON_SUCCESS;
}
/**
 * @description: xp_turners_stop Emergency stop
 * @param void
 * @return: 
 *      0  success
 *     -1  fail
 */
int xp_turners_stop()
{
    int ret = 0 ;
    xp_turners_get_encode();
    ret = synron_position_setvalue(turners.hinge_hander,0,0);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("xp_turners_stop fail !!!");
        xp_io_write_pin(T_BOARD_ID,T_INPUT_PIN,T_HIGH_STATE);//通信有问题紧急输入制动信号
        return SYNRON_COMMUN_ERR;
    }
    xp_turners_get_encode();
    ifturnstop = 1;
    return SYNRON_SUCCESS;
     
}


/**
 * @description: xp_turners_get_encode
 * @param void
 * @return: 
 *          absolute pulse 
 */

int xp_turners_get_encode()
{
    int encode = synron_get_abs_pulsevalue(turners.hinge_hander); 
    if(encode == READADD_32_ERR)
    {
        LOG_SYNRON("xp_turners_get_encode error!!");
        xp_io_write_pin(T_BOARD_ID,T_INPUT_PIN,T_HIGH_STATE);//通信有问题紧急输入制动信号
        return READADD_32_ERR;
    }
    turners.encode =encode;
    turners.angle = xp_turners_convert_pulsevaule(encode);
    return encode;
    
}

/**
 * @description: Convert the pulse into an Angle
 * @param {type} pulsevaule 
 * @return: 
 *          angle  unit  degree notice it's not radians
 absolute angle is 90.000000 and encode is 250000 
 */
double xp_turners_convert_pulsevaule(s32 pulsevaule)
{
    return ((double)(pulsevaule*360.00/(TURNERS_part*TURNERS_ratio)));

}

/**
 * @description:  xp_turners_check_zero 
 * @param {type}    void
 * @return: *      
 *      0  success
 *     -1  fail
 *      
 */
int xp_turners_check_zero(void)
{
    int ret = 0;
    xp_turners_get_encode();
    LOG_SYNRON("absolute angle is %f and encode is %d ",turners.angle,turners.encode);
    /*
        conditions 
    */
    for(int i =0; i<2;i++)
    {
        ret = synron_position_re_encode_setvalue(turners.hinge_hander,0);
        if(ret != SYNRON_SUCCESS)
        {
            LOG_SYNRON(" synron_position_re_setvalue !!");
            return SYNRON_COMMUN_ERR;
        }
        ret = synron_position_re_locacmd_setvalue(turners.hinge_hander,0);
        if(ret != SYNRON_SUCCESS)
        {
            LOG_SYNRON(" synron_position_re_setvalue !!");
            return SYNRON_COMMUN_ERR;
        }
        aos_msleep(50);
    }
    LOG_SYNRON("reset absolute angle is %f and encode is %d ",turners.angle,turners.encode);
    return SYNRON_SUCCESS;
}

/**
 * @description:  xp_turners_set_cur_angle 
 * @param {type}    angle set specific angle
 * @return: *      
 *      0  success
 *     -1  fail
 *      
 */
int xp_turners_set_cur_angle(double angle)
{
    int ret = 0;
    xp_turners_get_encode();
    LOG_SYNRON("absolute angle is %f and encode is %d ",turners.angle,turners.encode);
    /*
        conditions 
    */
    s32 cur_code = xp_turners_convert_angle(angle);
    for(int i = 0 ;i<2;i++)
    {
        ret = synron_position_re_encode_setvalue(turners.hinge_hander,cur_code);
        if(ret != SYNRON_SUCCESS)
        {
            LOG_SYNRON(" synron_position_re_setvalue !!");
            return SYNRON_COMMUN_ERR;
        }
        ret = synron_position_re_locacmd_setvalue(turners.hinge_hander,cur_code);
        if(ret != SYNRON_SUCCESS)
        {
            LOG_SYNRON(" synron_position_re_setvalue !!");
            return SYNRON_COMMUN_ERR;
        }
        LOG_SYNRON("reset absolute angle is %f and encode is %d ",angle,cur_code);
    }
    return SYNRON_SUCCESS;
}


/*
 * @description: xp_turners_acc_dcc_time
* @param param:type: 
*              param:times   Acceleration and deceleration time unit mm
 * @return: 
 */
int xp_turners_acc_dcc_time(u16 times)
{
    int ret = 0;
    synro_brush_hinge_hander.config.acc_time = times;
    synro_brush_hinge_hander.config.dcc_time = times;
     ret = synron_speedmode_acceleratetime(&synro_brush_hinge_hander,synro_brush_hinge_hander.config.acc_time);
    if(ret != 0)
   {
       LOG_SYNRON("[%s_%d]  synron_speedmode_acceleratetime(&synro_handler_A) fail!!!",__FILENAME__,__LINE__);           
       return SYNRON_COMMUN_ERR;
   }
    ret = synron_speedmode_deceleratetime(&synro_brush_hinge_hander,synro_brush_hinge_hander.config.dcc_time);
    if(ret != 0)
   {
       LOG_SYNRON("[%s_%d]  synron_speedmode_deceleratetime(&synro_handler_A) fail!!!",__FILENAME__,__LINE__);           
       return SYNRON_COMMUN_ERR;
   }
    return 0;
}

/**
 * @description:  xp_turners_find_wait_positon 
 * @param {type}   void
 * @return: *      
 *      0  success
 *     -1  fail
 *      
 */
int xp_turners_find_wait_positon()
{
    int ret = 0;
    double angle_limit = -100;
    u16 motor_speed = 0;
    int pulse_d = 0;
    pulse_d = xp_turners_convert_angle(angle_limit);
    LOG_SYNRON("des_pulse %d  angle %f",pulse_d,angle_limit);
    ret = xp_turners_check_zero();
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("xp_turners_find_wait_positon fail ");
        return SYNRON_COMMUN_ERR;
    }
    motor_speed = (u16)(1.0*(TURNERS_LIFTS_MOTORS));
    ret = synron_position_setvalue(turners.hinge_hander,pulse_d,motor_speed);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("synron_position_setvalue turners.hinge_hander  error ! ");
        xp_turners_error_anction();
        return SYNRON_COMMUN_ERR;
    }
   ifturnstop = 0;
   while(1)
   {
         if(ifturnstop == 1)
        {
            LOG_SYNRON("stop touched !!!");
            ifturnstop = 0;
            return SYNRON_NOR_ERR;
        }
        if(READADD_32_ERR==xp_turners_get_encode())
        {
           xp_turners_error_anction();
           LOG_SYNRON("communication fail");
           return SYNRON_COMMUN_ERR;
        }
        if(xp_io_read_input_pin(1,TURNERS_WAIT_POSITION)==0)
        {
             ret = xp_turners_stop();
             if(ret != SYNRON_SUCCESS)
             {
               xp_turners_error_anction();
             }
             ret = xp_turners_check_zero();
             if(ret != SYNRON_SUCCESS)
             {
                 LOG_SYNRON("communication fail ");
                 return SYNRON_COMMUN_ERR;
             }
            LOG_SYNRON("xp_turners_find_wait_positon sucess");
            return SYNRON_SUCCESS;
        }
        if(xp_io_read_input_pin(1,TURNERS_DOWN_LIMIT_POSITION)==0)
        {
              ret = xp_turners_stop();
             if(ret != SYNRON_SUCCESS)
             {
               xp_turners_error_anction();
             }
             LOG_SYNRON("reach the down position");
             ret = xp_turners_set_cur_angle(TURNERS_MIX_ANGLE);
             if(ret != SYNRON_SUCCESS)
             {
                 LOG_SYNRON("communication fail ");
                 return SYNRON_COMMUN_ERR;
             }
             break;
        }
        aos_msleep(150);
        LOG_SYNRON("des_angle %f  and  cur_angle %f",angle_limit,turners.angle);
   }
   angle_limit = 20;
   pulse_d = xp_turners_convert_angle(angle_limit);
   LOG_SYNRON("des_pulse %d  angle %f",pulse_d,angle_limit);
   ifturnstop = 0;
   ret = synron_position_setvalue(turners.hinge_hander,pulse_d,motor_speed);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("synron_position_setvalue turners.hinge_hander  error ! ");
        xp_turners_error_anction();
        return SYNRON_COMMUN_ERR;
    }
   while(1)
   {
          if(ifturnstop == 1)
        {
            LOG_SYNRON("stop touched !!!");
            ifturnstop = 0;
            return SYNRON_NOR_ERR;
        }
        if(READADD_32_ERR==xp_turners_get_encode())
        {
           xp_turners_error_anction();
           LOG_SYNRON("communication fail");
           return SYNRON_COMMUN_ERR;
        }
        if(fabs(turners.angle-angle_limit)<1)
        {
          LOG_SYNRON("lost and can not find wait position");  
          return SYNRON_NOR_ERR;
        }
        if(xp_io_read_input_pin(1,TURNERS_WAIT_POSITION)==0)
        {
             ret = xp_turners_stop();
             if(ret != SYNRON_SUCCESS)
             {
               xp_turners_error_anction();
             }
             ret = xp_turners_check_zero();
             if(ret != SYNRON_SUCCESS)
             {
                 LOG_SYNRON("communication fail ");
                 return SYNRON_COMMUN_ERR;
             }
            LOG_SYNRON("xp_turners_find_wait_positon sucess secondly");
            return SYNRON_SUCCESS;
        }
     LOG_SYNRON("des_angle %f  and  cur_angle %f",angle_limit,turners.angle);
     aos_msleep(150);
   }
   return SYNRON_COMMUN_ERR;

}

void xp_turners_error_anction()
{
    LOG_SYNRON("some bad thing happen xp_turners_error_anction");
    xp_io_output_set(1, TURN_POWWER,0);
}




/*
 * @description: xp_turners_debug
* @param param:type: 
*              fun function 
*              param     
 * @return: 
 */


int xp_turners_debug(char *type,char *fun,char *param)
{
    int ret =0;
    double agnle =0;
    u16 times_ms = 0;
    u16 speedprm = 0;
    if(type == NULL)
    {
        LOG_SYNRON("type is null error !!");
        return 0;
    }
    if(strcmp(type,"turners") == 0)
    {
        if(strcmp(fun,"init") == 0)
        {
            LOG_SYNRON("debug xp turners will initialize");
            ret = xp_turners_init(1,115200,SYNRON_BRUSH_HINGE_SLAVEID);//debug 模式 SYNRON_A_MOTOR_SLAVEID 临时代替
            if(ret != SYNRON_SUCCESS)
            {
                LOG_SYNRON("xp_turners_init fail!!");
                return 1;
            }
            LOG_SYNRON("xp_turners_init success!");
            return 1;
        }
        else if(strcmp(fun,"stop") == 0)
        {
            ret = xp_turners_stop();
            if(ret != SYNRON_SUCCESS)
            {
                LOG_SYNRON("xp_turners_stop  faile!!");
                xp_turners_error_anction();
                return 1;
            }
            LOG_SYNRON("xp_turners_stop start !!");
             return 1;
        }
        else if(strcmp(fun,"locate") == 0)
        {
            xp_turners_get_encode();
            LOG_SYNRON("absolute angle is %f and encode is %d ",turners.angle,turners.encode);
            return 1;
        }
        else if(strcmp(fun,"check") == 0)
        {
            ret = xp_turners_check_zero();
             return 1;
        }
        else if(strcmp(fun,"set_angle") == 0)
        {
            agnle = atof(param);
            LOG_SYNRON("set angle is %f",agnle);
            ret = xp_turners_set_cur_angle(agnle);
            return 1;
        }
        else if(strcmp(fun,"findz") == 0)
        {
           LOG_SYNRON("find the wati location");
           ret = xp_turners_find_wait_positon();
           if(ret != 0)
           {
                LOG_SYNRON("fail !!");
           }
           LOG_SYNRON("reacn the wait position ");
        }
        else if(strcmp(fun,"accdcc") == 0)
        {
            times_ms = strtoul(param, NULL, 10);
            LOG_SYNRON("set times_ms is %d \n",times_ms);
            ret = xp_turners_acc_dcc_time(times_ms);
            if(ret != SYNRON_SUCCESS)
            {
                LOG_SYNRON("xp_turners_acc_dcc_time fail !");
                return SYNRON_COMMUN_ERR;
            }
            return 1;
        }
        else
        {
            return 0;
        }
       
    }
    if(strcmp(type,"turners_s") == 0)
    {
        agnle = atof(fun);
        speedprm = strtoul(param, NULL, 10);
        LOG_SYNRON("angle is %f and speedprm is %d",agnle,speedprm);
        ret = xp_turners_move_speed(agnle,speedprm);
        if(ret != SYNRON_SUCCESS)
        {
            LOG_SYNRON("xp_turners_move fail!");
            return SYNRON_COMMUN_ERR;
        }
        LOG_SYNRON("xp_turners_move success!");
        return 1;
    }
    else if (strcmp(type,"turners_m") == 0)
    {
        agnle = atof(fun);
        times_ms = strtoul(param, NULL, 10);
        LOG_SYNRON("angle is %f and times_ms is %d",agnle,times_ms);
        ret = xp_turners_move_time(agnle,times_ms);
        if(ret != SYNRON_SUCCESS)
        {
            LOG_SYNRON("xp_turners_move fail!");
            return SYNRON_COMMUN_ERR;
        }
        LOG_SYNRON("xp_turners_move success!");
        return 1;
    }
    else
    {
        return 0;
    }
    
}
